home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 46 / Amiga Format CD46 (1999-10-20)(Future Publishing)(GB)[!][issue 1999-12].iso / -in_the_mag- / reader_requests / scilab / demos / npend / maple / npend.map < prev    next >
Text File  |  1999-09-16  |  9KB  |  288 lines
  1.  
  2. read(`Euler.map`);
  3. #----------------------------------------------------------------------------
  4. # AND NOW THE N-LINK PNDULUM
  5. #-----------------------------------------------------------------------------
  6.  
  7. #----------------------------------------------------------------------------
  8. # TeX Notations for my problem ( x= `x` is in fact useless ut just help
  9. #                to remind the state names )
  10. #-----------------------------------------------------------------------------
  11.  
  12. addnotations( { x = `x`  , th = `\\theta`, y =`y` , t = `\\eta`} );
  13.  
  14. #----------------------------------------------------------------------------
  15. # Lagrangian for My problem : the N-link pendulum 
  16. # l[i] : length of links  r[i]:=l[i]/2;
  17. #---------------------------------------------------------------------------
  18.  
  19. # number of link in the pendulum 
  20. n:=10;
  21.  
  22. # [x,y,theta] is of size three
  23. mm:=3: 
  24.  
  25. Li:=proc(i)
  26.     (1/2)*m[i]*( ( xd[i-1]- r[i] *sin(th[i])*thd[i])**2 + 
  27.     ( yd[i-1]+ r[i]*cos(th[i])*thd[i])**2 ) + 1/2*J[i]*(thd[i])**2
  28.     -m[i]*g*(y[i-1]+r[i]*sin(th[i])):
  29. end:
  30.  
  31. # The point zero is fixed 
  32.  
  33. x[0]:=0:xd[0]:=0:
  34. y[0]:=0:yd[0]:=0:
  35.  
  36. L:=sum( Li(j),j=1..n):
  37.  
  38. sorties(`systeme.tex`,`Lagrangian`,L):
  39.  
  40. # Lagrangian variables :
  41. q := [ seq (op([x[i],y[i],th[i]]),i=1..n)]:
  42. qd := [ seq (op([xd[i],yd[i],thd[i]]),i=1..n)]:
  43. qdd:= [ seq (op([xdd[i],ydd[i],thdd[i]]),i=1..n)]:
  44.  
  45. # Lhs of Euler equations 
  46.  
  47. EL:=euler_equations(L,q,qd,qdd):
  48. E:=map((i)->rhs(i),EL):
  49. sortiesM(`systeme.tex`,`Lhs of Euler Equations`,EL):
  50.  
  51. #-----------------------------------------------------
  52. # Rewritting the Euler equations to have a canonical form 
  53. # for TeX output
  54. #           ..         .          .
  55. # El= ME(q)  q + CE(q) q^2 + RE(q,q)
  56. # x^2 means transpose( x[1]^2 ,....,x[n]^2)
  57. # Computation of ME,CE,RE 
  58. #-----------------------------------------------------
  59.  
  60. XX:=CEuler(E,q,qd,qdd):
  61.  
  62. # simplifying notations for output
  63.  
  64. ME1:=XX[1]:
  65. ME1:=subs(seq(m[k]*r[k]*cos(th[k])=mrc[k],k=1..n),
  66.     seq(m[k]*r[k]*sin(th[k])=mrs[k],k=1..n),eval(ME1)):
  67.  
  68. sortiesI(`systeme.tex`,`$mrc_k=m_k r_k \\cos(\\theta_k)$`);
  69. sortiesI(`systeme.tex`,`$mrs_k=m_k r_k \\sin(\\theta_k)$`);
  70. sorties(`systeme.tex`,`$M(q)\\ddot{q}+C(q)\\dot{q}^2 +R(q,\\dot{q})$,M:`,
  71.     eval(ME1)):
  72. sorties(`systeme.tex`,`C~:`,XX[2]):
  73. sorties(`systeme.tex`,`R~:`,XX[3]):
  74. #------------------------------------------------
  75. # Constraints on the N-link Pendulum 
  76. #------------------------------------------------
  77. ncont:=2*n;
  78. cont:=[ seq(op([ x[i]-x[i-1] -2*r[i]*cos(th[i]),
  79.         y[i]-y[i-1] -2*r[i]*sin(th[i])]),i=1..n)]:
  80.  
  81. sortiesM(`systeme.tex`,`Constraints `,cont);
  82.  
  83. # time derivative of constraints;
  84.  
  85. cont1:=map ( (exp)->( time_diff(exp,q,qd,qdd)),cont):
  86.  
  87. # Derivatives of constraints are of type Aprim qd = 0 
  88.  
  89. Aprim:=genmatrix(cont1,qd):
  90.  
  91. sorties(`systeme.tex`,`Time derivative of constraints : $A'(q)\\dot{q}$`,Aprim);
  92.  
  93. #---------
  94. # Total Energy Ec + Ep 
  95. #---------
  96.  
  97. Ei:=proc(i)
  98. (1/2)*m[i]*( ( xd[i-1]- r[i] *sin(th[i])*thd[i])**2 + 
  99.     ( yd[i-1]+ r[i]*cos(th[i])*thd[i])**2 ) + 1/2*J[i]*(thd[i])**2
  100.     +m[i]*g*(y[i-1]+r[i]*sin(th[i])):
  101. end:
  102.  
  103. ET:=sum( Ei(j),j=1..n):
  104. Scont1:=solve({op(cont1)},{ seq (op([xd[i],yd[i]]),i=1..n)}):
  105. Scont2:=solve({op(cont)},{ seq (op([x[i],y[i]]),i=1..n)}):
  106. ET:=subs(Scont1,ET):
  107. ET:=subs(Scont2,ET):
  108.  
  109. sorties(`systeme.tex`,`N-pendulum energy~:`,ET);
  110.  
  111. #----------------------------------------------
  112. # Computing SS:=S(q);
  113. # S(q) will solve Aprim S(q) = 0 
  114. # in The Euler equations  Equ= A(q)' lambda + u 
  115. # the term A(q)lambda can be eliminated 
  116. # if we left-multiply euler equations by S(q)'
  117. #----------------------------------------------
  118.  
  119. SS:=linsolve(Aprim,matrix(ncont,1,0)):
  120.  
  121. #------------------------------------------------------------------------
  122. # The constraints are now dotq=S(q)eta 
  123. # can be used to see that eta[i]=thd[i] ( eta[i] is the maple variable ti )
  124. # but the indices can be mixed and linsolve doesn't 
  125. # always return the same result 
  126. # We have to check the correspondance between t.sig(i)=thd[i]
  127. # and to change SS to have a good corespondance 
  128. #-------------------------------------------------------------------------
  129.  
  130. permut:={seq(SS[mm*i,1]=t_s[i],i=1..n)}:
  131. SS:=subs(permut,eval(SS)):
  132. permut:={seq(t_s[i]=t[i],i=1..n)}:
  133. SS:=subs(permut,eval(SS)):
  134.  
  135. S:= genmatrix(convert(convert(SS,vector),list),[seq( t[i],i=1..n)]):
  136. sorties(`systeme.tex`,`$\\dot{q}=S(q)\\eta$ Kernel of $A(q)'$~:`,S):
  137.  
  138. #-----------------------------------------------------
  139. # this multiplication eliminates the term A(q) lambda 
  140. # in the Euler equations 
  141. #-----------------------------------------------------
  142.  
  143. E1:=multiply(transpose(S),E):
  144.  
  145. # sortiesM(`systeme.tex`,`$S(q)^T E$`,E1);
  146.  
  147. #-----------------------------------------------------
  148. # since Aprim(q) dotq=0 
  149. # .
  150. # q= S(q) eta ; here  eta = [t1,t2]
  151. #                                 ..
  152. # we use this equation to compute q
  153. # Warning : t1 and t2 are time dependent
  154. #-----------------------------------------------------
  155.  
  156. qt  := [ seq (t[i]  ,i=1..n)]:
  157. qtd := [ seq (td[i] ,i=1..n)]:
  158. qtdd:= [ seq (tdd[i],i=1..n)]:
  159.  
  160. qqdd:=map((x,y,z,t)-> time_diff(x,y,z,t),eval(SS),
  161.     [op(q),op(qt)],[op(qd),op(qtd)],[op(qdd),op(qtdd)]):
  162.  
  163. #-----------------------------------------------------
  164. #       ..                       .
  165. # using  q= d/dt [ S(q) eta] and q= S(q) eta 
  166. # we can subsitute these expressions in E1
  167. #-----------------------------------------------------
  168.  
  169. E2:=subs(seq(qdd[i]=qqdd[i,1],i=1..nops(qdd)),eval(E1)):
  170. E3:=subs(seq(qd[i]=SS[i,1],i=1..nops(qd)),eval(E2)):
  171.  
  172. #-----------------------------------------------------
  173. # The global system is now 
  174. #             .
  175. #  E3 = 0 and q= S(q) eta 
  176. #-----------------------------------------------------
  177.  
  178. E3:=map((x)-> simplify(x),E3):
  179.  
  180. sortiesM(`systeme.tex`,
  181.     `$S(q)^T E$ simplified with $\\dot{q}=S(q)\\eta $`,E3);
  182.  
  183. #------------------------------------------------------------------
  184. # Trying to find canonical representation 
  185. # for the simplified euler equations
  186. #            .
  187. # El= ME(q)  t + CE(q) t^2 + RE
  188. # we use CEuler with a little trick in the parameter call qt,qt,qtd
  189. #------------------------------------------------------------------
  190.  
  191. XX1:=CEuler(E3,qt,qt,qtd):
  192.  
  193. MM3:=map((i)->factor(combine(i,trig)),XX1[1]):
  194. CC3:=map((i)->factor(combine(i,trig)),XX1[2]):
  195. RR3:=map((i)->factor(combine(i,trig)),XX1[3]):
  196.  
  197. sorties(`systeme.tex`,`a cononical form $M(q)\\dot{t}+C(q)t^2 +R$,M:`,MM3);
  198. sorties(`systeme.tex`,`C:`,CC3);
  199. sorties(`systeme.tex`,`R:`,RR3);
  200.  
  201. sortiesI(`systeme.tex`,`Since $M,C,R$ only depends on $\\theta$ `);
  202. sortiesI(`systeme.tex`,`,we can forget $x_i,y_i$ in the dynamical system `);
  203. sortiesI(`systeme.tex`,` and just keep the $\\dot{\theta}=\\eta$ in the    constraints`);
  204.  
  205. #-----------------------------------------------------
  206. # FORTRAN GENERATION 
  207. #-----------------------------------------------------
  208.  
  209. #-----------------------------------------------------
  210. # First routine npend(neq,t,th,ydot)
  211. #                     .
  212. # we have MM3(theta) eta + CC3(theta)*eta^2 +RR3(theta) = 0 
  213. #   .
  214. # theta = eta
  215. #                           .
  216. # ==> th=[ theta,eta] ydot= th
  217. #-----------------------------------------------------
  218.  
  219.  
  220. flist:=[subroutinem,`npend`,[`neq`,`t`,`th`,`ydot`],
  221.            [
  222.             [ parameterf,[`n=`.n]],
  223.             [ declaref,doubleprecision,[`t,th(2*n),ydot(2*n),r(n),j(n),m(n)`]],
  224.             [ declaref,doubleprecision,[`me3s(n,n),cc3s(n,n),const(n,1)`]],
  225.             [ declaref,doubleprecision,[`w(3*n),rcond    `]],
  226.             [ declaref,`implicit doubleprecision`,[`(t)`] ],
  227.             [ declaref,integer,[`i,k,neq,ierr`]],
  228.             [ declaref,`data g`,[`/ 9.81/`]],
  229.             [ declaref,`data r`,[`/ n*1.0/`] ],
  230.             [ declaref,`data m`,[`/ n*1.0/`] ],
  231.             [ declaref,`data j`,[`/ n*0.3/`] ],
  232.             [ matrixm,`me3s`,MM3 ] ,
  233.             [ matrixm,`cc3s`,CC3 ] ,
  234.             [ matrixm,`const`,RR3 ] ,
  235.         [ dom , `i ` ,1,`n `,1,[ equalf,`ydot(i)`,`th(i+n)`]],
  236.         [ dom , `i ` ,1,`n `,1,[
  237.         [ dom , `k ` ,1,`n `,1,
  238.             [[ equalf,`Const(i,1)`,
  239.                 ` Const(i,1)+CC3S(i,k)*(th(k+n)**2)`]]],
  240.         [ equalf,` Const(i,1)`,`-Const(i,1)`]]],
  241.         [commentf,` we must solve  M z =const `],
  242.         [commentf,` which gives ydot((n+1)..2*n) `],
  243.         [ callf , `dlslv`,[`me3s,n,n,Const,n,1,w, rcond,ierr,1`]],
  244.         [ if_then_m,ierr<>0,[
  245.         [writef,6,ff_w,[]],
  246.         [formatf,ff_w,[`'Matrice mal conditionnee'`]]]],
  247.         [ dom , `i ` ,1,`n `,1,[ equalf,`ydot(n+i)`,`const(i,1)`]],
  248.         [returnf]]]:
  249.  
  250.  
  251. Gener(`npend.f`,flist):
  252.  
  253. # New ener.f
  254. ET:=matrix(1,1,[ET]):
  255.  
  256. flist:=[subroutinem,`ener`,[`th`,`e`],
  257.            [
  258.             [ parameterf,[`n=`.n]],
  259.             [ declaref,doubleprecision,[`th(2*n),thd(n),et(1,1),r(n),j(n),m(n)`]],
  260.             [ declaref,`implicit doubleprecision`,[`(t)`] ],
  261.             [ declaref,integer,[`i `]],
  262.             [ declaref,`data g`,[`/ 9.81/`]],
  263.             [ declaref,`data r`,[`/ n*1.0/`] ],
  264.             [ declaref,`data m`,[`/ n*1.0/`] ],
  265.             [ declaref,`data j`,[`/ n*0.3/`] ],
  266.         [ dom , `i ` ,1,`n `,1,[ equalf,`thd(i)`,`th(i+n)`]],
  267.             [ matrixm,`et`,ET ] ,
  268.         [ equalf,`e`,`et(1,1)`],
  269.         [returnf]]]:
  270.  
  271. Gener(`ener.f`,flist):
  272.  
  273. # New np.f 
  274.  
  275. flist:=[subroutinem,`np`,[`i `],
  276.            [
  277.             [ declaref,integer,[`i `]],
  278.         [ equalf,`i `,n],
  279.         [returnf]]]:
  280.  
  281. Gener(`np.f`,flist):
  282.  
  283.  
  284.  
  285.  
  286.  
  287.  
  288.